<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="utf-8">
<title>demos/VortexRigidBodyPlanning.cpp Source File</title>
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<meta name="author" content="Ioan A. Șucan, Mark Moll, Lydia E. Kavraki">

<link href="http://ompl.kavrakilab.org/css/tabs.css" rel="stylesheet">
<link href="http://ompl.kavrakilab.org/css/doxygen.css" rel="stylesheet">
<link href="http://ompl.kavrakilab.org/css/search.css" rel="stylesheet">
<link href="http://ompl.kavrakilab.org/css/bootstrap.css" rel="stylesheet">
<link href="http://ompl.kavrakilab.org/css/bootstrap.responsive.css" rel="stylesheet">
<link href="http://ompl.kavrakilab.org/css/ompl.css" rel="stylesheet">

<!--[if lt IE 9]>
  <script src="http://html5shim.googlecode.com/svn/trunk/html5.js"></script>
<![endif]-->

</head>
<body style="padding-top: 0px">
<div class="container"><div>
<div class="contents">
<div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*********************************************************************</span>
<a name="l00002"></a>00002 <span class="comment">* Software License Agreement (BSD License)</span>
<a name="l00003"></a>00003 <span class="comment">*</span>
<a name="l00004"></a>00004 <span class="comment">*  Copyright (c) 2010, Rice University</span>
<a name="l00005"></a>00005 <span class="comment">*  All rights reserved.</span>
<a name="l00006"></a>00006 <span class="comment">*</span>
<a name="l00007"></a>00007 <span class="comment">*  Redistribution and use in source and binary forms, with or without</span>
<a name="l00008"></a>00008 <span class="comment">*  modification, are permitted provided that the following conditions</span>
<a name="l00009"></a>00009 <span class="comment">*  are met:</span>
<a name="l00010"></a>00010 <span class="comment">*</span>
<a name="l00011"></a>00011 <span class="comment">*   * Redistributions of source code must retain the above copyright</span>
<a name="l00012"></a>00012 <span class="comment">*     notice, this list of conditions and the following disclaimer.</span>
<a name="l00013"></a>00013 <span class="comment">*   * Redistributions in binary form must reproduce the above</span>
<a name="l00014"></a>00014 <span class="comment">*     copyright notice, this list of conditions and the following</span>
<a name="l00015"></a>00015 <span class="comment">*     disclaimer in the documentation and/or other materials provided</span>
<a name="l00016"></a>00016 <span class="comment">*     with the distribution.</span>
<a name="l00017"></a>00017 <span class="comment">*   * Neither the name of the Rice University nor the names of its</span>
<a name="l00018"></a>00018 <span class="comment">*     contributors may be used to endorse or promote products derived</span>
<a name="l00019"></a>00019 <span class="comment">*     from this software without specific prior written permission.</span>
<a name="l00020"></a>00020 <span class="comment">*</span>
<a name="l00021"></a>00021 <span class="comment">*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS</span>
<a name="l00022"></a>00022 <span class="comment">*  &quot;AS IS&quot; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT</span>
<a name="l00023"></a>00023 <span class="comment">*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS</span>
<a name="l00024"></a>00024 <span class="comment">*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE</span>
<a name="l00025"></a>00025 <span class="comment">*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,</span>
<a name="l00026"></a>00026 <span class="comment">*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,</span>
<a name="l00027"></a>00027 <span class="comment">*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;</span>
<a name="l00028"></a>00028 <span class="comment">*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER</span>
<a name="l00029"></a>00029 <span class="comment">*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT</span>
<a name="l00030"></a>00030 <span class="comment">*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN</span>
<a name="l00031"></a>00031 <span class="comment">*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE</span>
<a name="l00032"></a>00032 <span class="comment">*  POSSIBILITY OF SUCH DAMAGE.</span>
<a name="l00033"></a>00033 <span class="comment">*********************************************************************/</span>
<a name="l00034"></a>00034 
<a name="l00035"></a>00035 <span class="comment">/* Author: Mark Moll */</span>
<a name="l00036"></a>00036 
<a name="l00037"></a>00037 <span class="preprocessor">#include &lt;ompl/extensions/vortex/VortexSimpleSetup.h&gt;</span>
<a name="l00038"></a>00038 <span class="preprocessor">#include &lt;ompl/base/GoalRegion.h&gt;</span>
<a name="l00039"></a>00039 
<a name="l00040"></a>00040 <span class="preprocessor">#include &lt;Vx/VxBox.h&gt;</span>
<a name="l00041"></a>00041 <span class="preprocessor">#include &lt;Vx/VxPart.h&gt;</span>
<a name="l00042"></a>00042 <span class="preprocessor">#include &lt;Vx/VxAssembly.h&gt;</span>
<a name="l00043"></a>00043 <span class="preprocessor">#include &lt;Vx/VxState.h&gt;</span>
<a name="l00044"></a>00044 
<a name="l00045"></a>00045 <span class="preprocessor">#include &lt;ompl/config.h&gt;</span>
<a name="l00046"></a>00046 <span class="preprocessor">#include &lt;iostream&gt;</span>
<a name="l00047"></a>00047 
<a name="l00048"></a>00048 <span class="keyword">namespace </span>ob = ompl::base;
<a name="l00049"></a>00049 <span class="keyword">namespace </span>og = ompl::geometric;
<a name="l00050"></a>00050 <span class="keyword">namespace </span>oc = ompl::control;
<a name="l00051"></a>00051 
<a name="l00053"></a>00053 
<a name="l00054"></a>00054 <span class="keyword">class </span>RigidBodyEnvironment : <span class="keyword">public</span> <a class="code" href="classompl_1_1control_1_1VortexEnvironment.html" title="This class contains the Vortex constructs OMPL needs to know about when planning.">oc::VortexEnvironment</a>
<a name="l00055"></a>00055 {
<a name="l00056"></a>00056 <span class="keyword">public</span>:
<a name="l00057"></a>00057 
<a name="l00058"></a>00058     RigidBodyEnvironment(<span class="keywordtype">void</span>) : oc::VortexEnvironment()
<a name="l00059"></a>00059     {
<a name="l00060"></a>00060         createWorld();
<a name="l00061"></a>00061     }
<a name="l00062"></a>00062     ~RigidBodyEnvironment()
<a name="l00063"></a>00063     {
<a name="l00064"></a>00064         visualizer_-&gt;stopViewer();
<a name="l00065"></a>00065     }
<a name="l00066"></a>00066 
<a name="l00067"></a>00067     <span class="comment">/**************************************************</span>
<a name="l00068"></a>00068 <span class="comment">     * Implementation of functions needed by planning *</span>
<a name="l00069"></a>00069 <span class="comment">     **************************************************/</span>
<a name="l00070"></a>00070 
<a name="l00071"></a>00071     <span class="keyword">virtual</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1control_1_1VortexEnvironment.html#a244e40f24267734653b22d5a3a34e531" title="Number of parameters (double values) needed to specify a control input.">getControlDimension</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00072"></a>00072 <span class="keyword">    </span>{
<a name="l00073"></a>00073         <span class="keywordflow">return</span> 3;
<a name="l00074"></a>00074     }
<a name="l00075"></a>00075 
<a name="l00076"></a>00076     <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1control_1_1VortexEnvironment.html#a4b41ee457635a2e657b691a31b98cf0a" title="Get the control bounds -- the bounding box in which to sample controls.">getControlBounds</a>(std::vector&lt;double&gt; &amp;lower, std::vector&lt;double&gt; &amp;upper)<span class="keyword"> const</span>
<a name="l00077"></a>00077 <span class="keyword">    </span>{
<a name="l00078"></a>00078         <span class="keyword">static</span> <span class="keywordtype">double</span> maxForce = 1.;
<a name="l00079"></a>00079         lower.resize(3);
<a name="l00080"></a>00080         lower[0] = -maxForce;
<a name="l00081"></a>00081         lower[1] = -maxForce;
<a name="l00082"></a>00082         lower[2] = 9.81 - maxForce;
<a name="l00083"></a>00083 
<a name="l00084"></a>00084         upper.resize(3);
<a name="l00085"></a>00085         upper[0] = maxForce;
<a name="l00086"></a>00086         upper[1] = maxForce;
<a name="l00087"></a>00087         upper[2] = 9.81 + maxForce;
<a name="l00088"></a>00088     }
<a name="l00089"></a>00089 
<a name="l00090"></a>00090     <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1control_1_1VortexEnvironment.html#a3f8afb9db4d7b375a30a3d561b7e79de" title="Application of a control. This function sets the forces/torques/velocities for bodies in the simulati...">applyControl</a>(<span class="keyword">const</span> <span class="keywordtype">double</span> *control)<span class="keyword"> const</span>
<a name="l00091"></a>00091 <span class="keyword">    </span>{
<a name="l00092"></a>00092         Vx::VxReal3 force = { control[0], control[1], control[2] };
<a name="l00093"></a>00093         <span class="keyword">static_cast&lt;</span>Vx::VxPart*<span class="keyword">&gt;</span>(assembly_-&gt;findEntityFromName(<span class="stringliteral">&quot;moving box&quot;</span>))-&gt;addForce(force);
<a name="l00094"></a>00094     }
<a name="l00095"></a>00095 
<a name="l00096"></a>00096     <span class="comment">/**************************************************/</span>
<a name="l00097"></a>00097 
<a name="l00098"></a>00098     <span class="comment">// OMPL does not require this function here; we implement it here</span>
<a name="l00099"></a>00099     <span class="comment">// for convenience. This function is only Vortex code to create a</span>
<a name="l00100"></a>00100     <span class="comment">// simulation environment. At the end of the function, there is a</span>
<a name="l00101"></a>00101     <span class="comment">// call to setPlanningParameters(), which configures members of</span>
<a name="l00102"></a>00102     <span class="comment">// the base class needed by planners.</span>
<a name="l00103"></a>00103     <span class="keywordtype">void</span> createWorld(<span class="keywordtype">void</span>);
<a name="l00104"></a>00104 
<a name="l00105"></a>00105     <span class="comment">// Set parameters needed by the base class (such as the bodies</span>
<a name="l00106"></a>00106     <span class="comment">// that make up to state of the system we are planning for)</span>
<a name="l00107"></a>00107     <span class="keywordtype">void</span> setPlanningParameters(<span class="keywordtype">void</span>);
<a name="l00108"></a>00108 
<a name="l00109"></a>00109     <span class="comment">// convenience function to get the position of the box</span>
<a name="l00110"></a>00110     <span class="keyword">const</span> Vx::VxVector3&amp; getPosition(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">ob::State</a>* st) <span class="keyword">const</span>;
<a name="l00111"></a>00111 };
<a name="l00112"></a>00112 
<a name="l00113"></a>00113 
<a name="l00114"></a>00114 <span class="comment">// Define the goal we want to reach</span>
<a name="l00115"></a>00115 <span class="keyword">class </span>RigidBodyGoal : <span class="keyword">public</span> <a class="code" href="classompl_1_1base_1_1GoalRegion.html" title="Definition of a goal region.">ob::GoalRegion</a>
<a name="l00116"></a>00116 {
<a name="l00117"></a>00117 <span class="keyword">public</span>:
<a name="l00118"></a>00118     RigidBodyGoal(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1SpaceInformationPtr.html" title="A boost shared pointer wrapper for ompl::base::SpaceInformation.">ob::SpaceInformationPtr</a> &amp;si) : ob::GoalRegion(si)
<a name="l00119"></a>00119     {
<a name="l00120"></a>00120         threshold_ = 1.;
<a name="l00121"></a>00121     }
<a name="l00122"></a>00122 
<a name="l00123"></a>00123     <span class="keyword">virtual</span> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1base_1_1GoalRegion.html#a4c7a9ab388d762c76711790af0ba53ea" title="Compute the distance to the goal (heuristic). This function is the one used in computing the distance...">distanceGoal</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">ob::State</a> *st)<span class="keyword"> const</span>
<a name="l00124"></a>00124 <span class="keyword">    </span>{
<a name="l00125"></a>00125         <span class="keyword">static</span> <span class="keyword">const</span> Vx::VxVector3 goalPos(30, 55, 35);
<a name="l00126"></a>00126         <span class="keyword">const</span> Vx::VxVector3&amp; pos =
<a name="l00127"></a>00127             si_-&gt;getStateSpace()-&gt;as&lt;<a class="code" href="classompl_1_1control_1_1VortexStateSpace.html" title="State manifold representing Vortex states.">oc::VortexStateSpace</a>&gt;()-&gt;getEnvironment()-&gt;<a class="code" href="classompl_1_1base_1_1StateSpace.html#a9c969c44aedd6949e98a61bb3d527a04" title="Cast this instance to a desired type.">as</a>&lt;RigidBodyEnvironment&gt;()-&gt;getPosition(st);
<a name="l00128"></a>00128         <span class="keywordflow">return</span> (goalPos - pos).norm();
<a name="l00129"></a>00129     }
<a name="l00130"></a>00130 };
<a name="l00131"></a>00131 
<a name="l00132"></a>00132 
<a name="l00133"></a>00133 <span class="comment">// Define how we project a state</span>
<a name="l00134"></a>00134 <span class="keyword">class </span>RigidBodyStateProjectionEvaluator : <span class="keyword">public</span> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluator.html" title="Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...">ob::ProjectionEvaluator</a>
<a name="l00135"></a>00135 {
<a name="l00136"></a>00136 <span class="keyword">public</span>:
<a name="l00137"></a>00137 
<a name="l00138"></a>00138     RigidBodyStateProjectionEvaluator(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html" title="Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.">ob::StateSpace</a> *space) : ob::ProjectionEvaluator(space)
<a name="l00139"></a>00139     {
<a name="l00140"></a>00140         cellSizes_.resize(3);
<a name="l00141"></a>00141         cellSizes_[0] = 1;
<a name="l00142"></a>00142         cellSizes_[1] = 1;
<a name="l00143"></a>00143         cellSizes_[2] = 1;
<a name="l00144"></a>00144     }
<a name="l00145"></a>00145 
<a name="l00146"></a>00146     <span class="keyword">virtual</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluator.html#a0db0f7ebd64179ae0d11531cbe23cb8a" title="Return the dimension of the projection defined by this evaluator.">getDimension</a>(<span class="keywordtype">void</span>)<span class="keyword"> const</span>
<a name="l00147"></a>00147 <span class="keyword">    </span>{
<a name="l00148"></a>00148         <span class="keywordflow">return</span> 3;
<a name="l00149"></a>00149     }
<a name="l00150"></a>00150 
<a name="l00151"></a>00151     <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1ProjectionEvaluator.html#a748a31a8f4e5737e93fcd74a430b0baf" title="Compute the projection as an array of double values.">project</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">ob::State</a> *state, <a class="code" href="namespaceompl_1_1base.html#aa83f2e90d56884b236b1e400d116b10b" title="The datatype for state projections. This class contains a real vector.">ob::EuclideanProjection</a> &amp;projection)<span class="keyword"> const</span>
<a name="l00152"></a>00152 <span class="keyword">    </span>{
<a name="l00153"></a>00153         <span class="keyword">const</span> Vx::VxVector3&amp; pos =
<a name="l00154"></a>00154             space_-&gt;as&lt;<a class="code" href="classompl_1_1control_1_1VortexStateSpace.html" title="State manifold representing Vortex states.">oc::VortexStateSpace</a>&gt;()-&gt;getEnvironment()-&gt;<a class="code" href="classompl_1_1base_1_1StateSpace.html#a9c969c44aedd6949e98a61bb3d527a04" title="Cast this instance to a desired type.">as</a>&lt;RigidBodyEnvironment&gt;()-&gt;getPosition(state);
<a name="l00155"></a>00155         projection[0] = pos[0];
<a name="l00156"></a>00156         projection[1] = pos[1];
<a name="l00157"></a>00157         projection[2] = pos[2];
<a name="l00158"></a>00158     }
<a name="l00159"></a>00159 
<a name="l00160"></a>00160 };
<a name="l00161"></a>00161 
<a name="l00162"></a>00162 <span class="comment">// Define our own state space, to include a distance function we want and register a default projection</span>
<a name="l00163"></a>00163 <span class="keyword">class </span>RigidBodyStateSpace : <span class="keyword">public</span> <a class="code" href="classompl_1_1control_1_1VortexStateSpace.html" title="State manifold representing Vortex states.">oc::VortexStateSpace</a>
<a name="l00164"></a>00164 {
<a name="l00165"></a>00165 <span class="keyword">public</span>:
<a name="l00166"></a>00166     RigidBodyStateSpace(<span class="keyword">const</span> <a class="code" href="classompl_1_1control_1_1VortexEnvironmentPtr.html" title="A boost shared pointer wrapper for ompl::control::VortexEnvironment.">oc::VortexEnvironmentPtr</a> &amp;env) : oc::VortexStateSpace(env)
<a name="l00167"></a>00167     {
<a name="l00168"></a>00168     }
<a name="l00169"></a>00169 
<a name="l00170"></a>00170     <span class="keyword">virtual</span> <span class="keywordtype">double</span> <a class="code" href="classompl_1_1control_1_1VortexStateSpace.html#acf61e4c1044a66068587400ae16e6ea5" title="Computes distance to between two states. This function satisfies the properties of a metric and its r...">distance</a>(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">ob::State</a> *s1, <span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">ob::State</a> *s2)<span class="keyword"> const</span>
<a name="l00171"></a>00171 <span class="keyword">    </span>{
<a name="l00172"></a>00172         <span class="keywordflow">return</span> (env_-&gt;as&lt;RigidBodyEnvironment&gt;()-&gt;getPosition(s1) 
<a name="l00173"></a>00173               - env_-&gt;as&lt;RigidBodyEnvironment&gt;()-&gt;getPosition(s2)).norm();
<a name="l00174"></a>00174     }
<a name="l00175"></a>00175 
<a name="l00176"></a>00176     <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="classompl_1_1base_1_1StateSpace.html#a18363a441aa3ec273b5a32bb9f9f21f3" title="Register the projections for this state space. Usually, this is at least the default projection...">registerProjections</a>(<span class="keywordtype">void</span>)
<a name="l00177"></a>00177     {
<a name="l00178"></a>00178         <a class="code" href="classompl_1_1base_1_1StateSpace.html#ac79f6342fb2d85f1c2c87bfff812fbbf" title="Register the default projection for this state space.">registerDefaultProjection</a>(<a class="code" href="classompl_1_1base_1_1ProjectionEvaluatorPtr.html" title="A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.">ob::ProjectionEvaluatorPtr</a>(<span class="keyword">new</span> RigidBodyStateProjectionEvaluator(<span class="keyword">this</span>)));
<a name="l00179"></a>00179     }
<a name="l00180"></a>00180 };
<a name="l00181"></a>00181 
<a name="l00183"></a>00183 
<a name="l00184"></a>00184 <span class="keywordtype">int</span> main(<span class="keywordtype">int</span>, <span class="keywordtype">char</span> **)
<a name="l00185"></a>00185 {
<a name="l00186"></a>00186     <span class="comment">// create the Vortex environment</span>
<a name="l00187"></a>00187     <a class="code" href="classompl_1_1control_1_1VortexEnvironmentPtr.html" title="A boost shared pointer wrapper for ompl::control::VortexEnvironment.">oc::VortexEnvironmentPtr</a> env(<span class="keyword">new</span> RigidBodyEnvironment());
<a name="l00188"></a>00188 
<a name="l00189"></a>00189     <span class="comment">// create the state space and the control space for planning</span>
<a name="l00190"></a>00190     <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">ob::StateSpacePtr</a> stateSpacePtr = <a class="code" href="classompl_1_1base_1_1StateSpacePtr.html" title="A boost shared pointer wrapper for ompl::base::StateSpace.">ob::StateSpacePtr</a>(<span class="keyword">new</span> RigidBodyStateSpace(env));
<a name="l00191"></a>00191 
<a name="l00192"></a>00192     <span class="comment">// this will take care of setting a proper collision checker and the</span>
<a name="l00193"></a>00193     <span class="comment">// starting state for the planner as the initial Vortex state</span>
<a name="l00194"></a>00194     <a class="code" href="classompl_1_1control_1_1VortexSimpleSetup.html" title="Create the set of classes typically needed to solve a control problem when forward propagation is com...">oc::VortexSimpleSetup</a> ss(stateSpacePtr);
<a name="l00195"></a>00195 
<a name="l00196"></a>00196     <span class="comment">// set the goal we would like to reach</span>
<a name="l00197"></a>00197     ss.setGoal(<a class="code" href="classompl_1_1base_1_1GoalPtr.html" title="A boost shared pointer wrapper for ompl::base::Goal.">ob::GoalPtr</a>(<span class="keyword">new</span> RigidBodyGoal(ss.getSpaceInformation())));
<a name="l00198"></a>00198 
<a name="l00199"></a>00199     <a class="code" href="classompl_1_1base_1_1RealVectorBounds.html" title="The lower and upper bounds for an Rn space.">ob::RealVectorBounds</a> bounds(3);
<a name="l00200"></a>00200     bounds.setLow(-1);
<a name="l00201"></a>00201     bounds.setHigh(70);
<a name="l00202"></a>00202     stateSpacePtr-&gt;as&lt;RigidBodyStateSpace&gt;()-&gt;setWorkspaceBounds(bounds);
<a name="l00203"></a>00203 
<a name="l00204"></a>00204 
<a name="l00205"></a>00205     ss.setup();
<a name="l00206"></a>00206     ss.print();
<a name="l00207"></a>00207     
<a name="l00208"></a>00208     <span class="comment">// ss.simulate(10);</span>
<a name="l00209"></a>00209     <span class="comment">// return 0;</span>
<a name="l00210"></a>00210 
<a name="l00211"></a>00211     <span class="keywordflow">if</span> (ss.solve(30))
<a name="l00212"></a>00212     {
<a name="l00213"></a>00213         <span class="keywordtype">bool</span> running;
<a name="l00214"></a>00214         <a class="code" href="classompl_1_1control_1_1PathControl.html" title="Definition of a control path.">oc::PathControl</a> p(ss.getSolutionPath());
<a name="l00215"></a>00215         <span class="comment">//p.interpolate(); // uncomment if you want to plot the path</span>
<a name="l00216"></a>00216 
<a name="l00217"></a>00217         <span class="comment">// print out box positions along the path</span>
<a name="l00218"></a>00218         <span class="keywordflow">for</span> (<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i &lt; p.states.size() ; ++i)
<a name="l00219"></a>00219         {
<a name="l00220"></a>00220             <span class="keyword">const</span> Vx::VxVector3 pos = env-&gt;as&lt;RigidBodyEnvironment&gt;()-&gt;getPosition(p.states[i]);
<a name="l00221"></a>00221             std::cout &lt;&lt; pos[0] &lt;&lt; <span class="charliteral">&#39; &#39;</span> &lt;&lt; pos[1] &lt;&lt; <span class="charliteral">&#39; &#39;</span> &lt;&lt; pos[2] &lt;&lt; <span class="charliteral">&#39; &#39;</span>;
<a name="l00222"></a>00222             <span class="keywordflow">if</span> (i==0)
<a name="l00223"></a>00223                 <span class="comment">// null controls applied for zero seconds to get to start state</span>
<a name="l00224"></a>00224                 std::cout &lt;&lt; <span class="stringliteral">&quot;0 0 0 0&quot;</span>;
<a name="l00225"></a>00225             <span class="keywordflow">else</span>
<a name="l00226"></a>00226             {
<a name="l00227"></a>00227                 <span class="comment">// print controls and control duration needed to get from state i-1 to state i</span>
<a name="l00228"></a>00228                 <span class="keyword">const</span> <span class="keywordtype">double</span>* c = p.controls[i-1]-&gt;as&lt;oc::RealVectorControlSpace::ControlType&gt;()-&gt;values;
<a name="l00229"></a>00229                 std::cout &lt;&lt; c[0] &lt;&lt; <span class="charliteral">&#39; &#39;</span> &lt;&lt; c[1] &lt;&lt; <span class="charliteral">&#39; &#39;</span> &lt;&lt; c[2] &lt;&lt;<span class="charliteral">&#39; &#39;</span> &lt;&lt; p.controlDurations[i-1];
<a name="l00230"></a>00230             }
<a name="l00231"></a>00231             std::cout &lt;&lt; std::endl;
<a name="l00232"></a>00232         }
<a name="l00233"></a>00233         <span class="comment">// loop over the box&#39;s motion</span>
<a name="l00234"></a>00234         <span class="keywordflow">do</span>
<a name="l00235"></a>00235         {
<a name="l00236"></a>00236             running = ss.playSolutionPath();
<a name="l00237"></a>00237         } <span class="keywordflow">while</span> (running);
<a name="l00238"></a>00238     }
<a name="l00239"></a>00239     <span class="keywordflow">return</span> 0;
<a name="l00240"></a>00240 }
<a name="l00241"></a>00241 
<a name="l00242"></a>00242 
<a name="l00243"></a>00243 
<a name="l00244"></a>00244 
<a name="l00245"></a>00245 
<a name="l00246"></a>00246 
<a name="l00248"></a>00248 
<a name="l00249"></a>00249 <span class="comment">/***********************************************</span>
<a name="l00250"></a>00250 <span class="comment"> * Member function implementations             *</span>
<a name="l00251"></a>00251 <span class="comment"> ***********************************************/</span>
<a name="l00252"></a>00252 
<a name="l00253"></a>00253 <span class="keywordtype">void</span> RigidBodyEnvironment::createWorld(<span class="keywordtype">void</span>)
<a name="l00254"></a>00254 {
<a name="l00255"></a>00255     <span class="comment">// BEGIN SETTING UP A VORTEX ENVIRONMENT</span>
<a name="l00256"></a>00256     <span class="comment">// ***********************************</span>
<a name="l00257"></a>00257     Vx::VxUniverse* universe = <span class="keyword">new</span> Vx::VxUniverse();
<a name="l00258"></a>00258     universe-&gt;setGravity(0., 0., -9.81);
<a name="l00259"></a>00259     world_-&gt;addUniverse(universe);
<a name="l00260"></a>00260 
<a name="l00261"></a>00261     visualizer_-&gt;addDataFilePathList(VORTEX_RESOURCE_DIR);
<a name="l00262"></a>00262     visualizer_-&gt;createDefaultWindow(0, NULL, <span class="stringliteral">&quot;Vortex Rigid Body Planning Demo&quot;</span>);
<a name="l00263"></a>00263     visualizer_-&gt;setUniverse(world_-&gt;getUniverse(0));
<a name="l00264"></a>00264     visualizer_-&gt;setFrameRateLock(<span class="keyword">true</span>);
<a name="l00265"></a>00265     visualizer_-&gt;createHelp();
<a name="l00266"></a>00266     visualizer_-&gt;toggleHelp();
<a name="l00267"></a>00267 
<a name="l00268"></a>00268     Vx::VxAssembly* assembly = <span class="keyword">new</span> Vx::VxAssembly();
<a name="l00269"></a>00269     Vx::VxPart* box = <span class="keyword">new</span> Vx::VxPart(1);
<a name="l00270"></a>00270     Vx::VxReal3 boxSize = {.2, .2, .1};
<a name="l00271"></a>00271     <span class="keywordtype">float</span> boxColor[4] = {1., .1, .1, 1.};
<a name="l00272"></a>00272 
<a name="l00273"></a>00273     box-&gt;setName(<span class="stringliteral">&quot;moving box&quot;</span>);
<a name="l00274"></a>00274     box-&gt;addGeometry(<span class="keyword">new</span> Vx::VxBox(boxSize), 0);
<a name="l00275"></a>00275     assembly-&gt;addEntity(box);
<a name="l00276"></a>00276     assembly_ = assembly; <span class="comment">// assembly_ contains all movable parts</span>
<a name="l00277"></a>00277     universe-&gt;addAssembly(assembly_);
<a name="l00278"></a>00278     Vx::VxNode boxNode = visualizer_-&gt;createBox(boxSize, boxColor);
<a name="l00279"></a>00279     box-&gt;setNode(boxNode);
<a name="l00280"></a>00280     box-&gt;setPosition(0., 0., 0.);
<a name="l00281"></a>00281     box-&gt;setOrientationQuaternion(1., 0., 0., 0.);
<a name="l00282"></a>00282 
<a name="l00283"></a>00283     visualizer_-&gt;setCameraLookAtAndPosition(Vx::VxVector3(30,55,20), Vx::VxVector3(-5,-5,-5));
<a name="l00284"></a>00284     <span class="comment">// *********************************</span>
<a name="l00285"></a>00285     <span class="comment">// END SETTING UP AN Vortex ENVIRONMENT</span>
<a name="l00286"></a>00286 
<a name="l00287"></a>00287     setPlanningParameters();
<a name="l00288"></a>00288 }
<a name="l00289"></a>00289 
<a name="l00290"></a>00290 <span class="keywordtype">void</span> RigidBodyEnvironment::setPlanningParameters(<span class="keywordtype">void</span>)
<a name="l00291"></a>00291 {
<a name="l00292"></a>00292     <span class="comment">// Fill in parameters for OMPL:</span>
<a name="l00293"></a>00293     stepSize_ = 0.05;
<a name="l00294"></a>00294     minControlSteps_ = 5;
<a name="l00295"></a>00295     maxControlSteps_ = 50;
<a name="l00296"></a>00296 }
<a name="l00297"></a>00297 
<a name="l00298"></a>00298 <span class="keyword">const</span> Vx::VxVector3&amp; RigidBodyEnvironment::getPosition(<span class="keyword">const</span> <a class="code" href="classompl_1_1base_1_1State.html" title="Definition of an abstract state.">ob::State</a>* st)<span class="keyword"> const</span>
<a name="l00299"></a>00299 <span class="keyword"></span>{
<a name="l00300"></a>00300     <span class="keyword">const</span> Vx::VxState* vxstate = st-&gt;<a class="code" href="classompl_1_1base_1_1State.html#acf404046d599ec27e8f3cea0bee4a9b3" title="Cast this instance to a desired type.">as</a>&lt;<a class="code" href="classompl_1_1base_1_1StateSpace.html#aa995357fedb73879bc7f0bb946db76d1" title="Define the type of state allocated by this space.">oc::VortexStateSpace::StateType</a>&gt;()-&gt;vxstate_;
<a name="l00301"></a>00301     vxstate-&gt;restoreState(assembly_);
<a name="l00302"></a>00302     <span class="keywordflow">return</span> <span class="keyword">static_cast&lt;</span>Vx::VxAssembly*<span class="keyword">&gt;</span>(assembly_)-&gt;getEntity(0)-&gt;getPosition();
<a name="l00303"></a>00303 }
<a name="l00304"></a>00304 
</pre></div></div><!-- contents -->
</div>

</body>
</html>
